#include <stdio.h>
#include <string.h>
#include "main.h"
#include "fdcan.h"
#include "bldc.h"
#include "pid.h"


FDCAN_HandleTypeDef hfdcan1;


// CAN接收消息队列
static can_rx_queue_t can_rx_queue = {0};

// 方向切换控制结构体
typedef struct {
    volatile uint8_t direction_change_pending;  // 方向切换待处理标志
    volatile motor_direction_t pending_direction;  // 待设置的方向  
    volatile uint32_t pending_speed;  // 待设置的速度
} direction_change_ctrl_t;

// 方向切换控制变量
static direction_change_ctrl_t direction_change_ctrl = {0};

static volatile uint8_t g_motor_start_request = 0;

// 统计信息
static volatile uint32_t can_msg_dropped_count = 0;



/**
  * @brief 入队CAN消息到接收队列
  * @param msg: 消息指针
  * @retval 1: 成功, 0: 失败(队列满)
  */
static uint8_t fdcan_queue_put_message(can_rx_queue_item_t *msg)
{
    if (can_rx_queue.count >= CAN_RX_QUEUE_SIZE)
    {
        can_msg_dropped_count++;
        return 0; // 队列满
    }
    
    // 拷贝消息到队列
    memcpy(&can_rx_queue.items[can_rx_queue.tail], msg, sizeof(can_rx_queue_item_t));
    
    can_rx_queue.tail = (can_rx_queue.tail + 1) % CAN_RX_QUEUE_SIZE;
    can_rx_queue.count++;
    
    return 1;
}

/**
  * @brief 从接收队列取出CAN消息
  * @param msg: 消息指针
  * @retval 1: 成功, 0: 队列空
  */
uint8_t fdcan_queue_get_message(can_rx_queue_item_t *msg)
{
    if (can_rx_queue.count == 0)
    {
        return 0; // 队列空
    }
    
    // 拷贝消息从队列
    memcpy(msg, &can_rx_queue.items[can_rx_queue.head], sizeof(can_rx_queue_item_t));
    
    can_rx_queue.head = (can_rx_queue.head + 1) % CAN_RX_QUEUE_SIZE;
    can_rx_queue.count--;
    
    return 1;
}

/**
  * @brief  获取电机启动请求（主循环调用）
  * @retval 1: 有启动请求, 0: 无启动请求
  * @note   此函数是原子操作，获取标志位后会自动清除，防止重复启动
  */
uint8_t fdcan_get_motor_start_request(void)
{
    if (g_motor_start_request == 1)
    {
        g_motor_start_request = 0; // 读取后立即清除，确保请求只被响应一次
        return 1;
    }
    return 0;
}

/**
  * @brief 获取方向切换状态（供bldc.c调用）
  * @param direction: 输出参数，返回待设置的方向
  * @param speed: 输出参数，返回待设置的速度
  * @retval 1: 有待处理的方向切换, 0: 无
  */
uint8_t fdcan_get_direction_change_request(uint8_t *direction, uint32_t *speed)
{
    if (direction_change_ctrl.direction_change_pending == 1)
    {
        *direction = direction_change_ctrl.pending_direction;
        *speed = direction_change_ctrl.pending_speed;
        return 1;
    }
    return 0;
}

/**
  * @brief 清除方向切换请求（供bldc.c调用）
  */
void fdcan_clear_direction_change_request(void)
{
    direction_change_ctrl.direction_change_pending = 0;
}

/**
  * @brief 紧急停止处理（在中断中立即执行）
  */
void fdcan_process_emergency_stop(void)
{
    // 立即停止电机，这是安全关键操作
    bldc_stop();
    // 关键：清除任何待处理的启动请求，确保STOP的最高优先级
    g_motor_start_request = 0;
    printf("EMERGENCY STOP executed!\r\n");
}

/**
  * @brief 在中断中快速处理目标速度
  * @param data: CAN数据
  */
void fdcan_process_target_speed_in_isr(uint8_t *data)
{
    can_target_speed_msg_t msg_data;
    
    // 快速解析消息数据
    msg_data.target_speed = (uint32_t)data[0] | 
                           ((uint32_t)data[1] << 8) |
                           ((uint32_t)data[2] << 16) |
                           ((uint32_t)data[3] << 24);
    msg_data.direction = data[4];
    //  printf("can speed: %d, dir: %d\r\n", msg_data.target_speed, msg_data.direction);
    if (msg_data.target_speed >= 0)
    {
        if(bldc_get_motor_status() == MOTOR_STOP) //停止状态时，直接设置方向和速度
        {
            bldc_set_motor_direction(msg_data.direction);
            pid_set_target_value(msg_data.target_speed);
        }
        else
        {
            if(msg_data.direction != bldc_get_motor_direction())
            {
                // 方向不同，需要换向
                direction_change_ctrl.direction_change_pending = 1;
                direction_change_ctrl.pending_direction = msg_data.direction;
                direction_change_ctrl.pending_speed = msg_data.target_speed;
                pid_set_target_value(0);  // 先减速到0
            }
            else
            {
                // 方向相同，直接设置速度
                pid_set_target_value(msg_data.target_speed);
            }
        }
       
    }
}

/**
  * @brief FDCAN1 Initialization Function
  * @param None
  * @retval None
  */
void fdcan_init(void)
{
    FDCAN_FilterTypeDef sFilterConfig;
    
    // 初始化接收队列
    memset(&can_rx_queue, 0, sizeof(can_rx_queue));
    
    hfdcan1.Instance = FDCAN1;
    hfdcan1.Init.ClockDivider = FDCAN_CLOCK_DIV1;
    hfdcan1.Init.FrameFormat = FDCAN_FRAME_CLASSIC;//传统模式，兼容can  
    hfdcan1.Init.Mode = FDCAN_MODE_NORMAL; 
    hfdcan1.Init.AutoRetransmission = DISABLE;
    hfdcan1.Init.TransmitPause = DISABLE;
    hfdcan1.Init.ProtocolException = DISABLE;

    //标称段，也就是仲裁段，在can中，所有段速度都一样，在fdcan中，data段速度可以不一样。
    //目前时钟为128mhz 128mhz/16 = 8mhz   8m/1+12+3 = 500kbs ，采样点为 （1+13）/16 = 0.875
    hfdcan1.Init.NominalPrescaler = 16;
    hfdcan1.Init.NominalSyncJumpWidth = 1;
    hfdcan1.Init.NominalTimeSeg1 = 13;
    hfdcan1.Init.NominalTimeSeg2 = 2;

    //数据段，在fdcan中才有用，在can中，速度和 naminal 一样。
    hfdcan1.Init.DataPrescaler = 1;
    hfdcan1.Init.DataSyncJumpWidth = 1;
    hfdcan1.Init.DataTimeSeg1 = 1;
    hfdcan1.Init.DataTimeSeg2 = 1;


    hfdcan1.Init.StdFiltersNbr = 3;                 // 使用3个标准ID过滤器
    hfdcan1.Init.ExtFiltersNbr = 0;
    hfdcan1.Init.TxFifoQueueMode = FDCAN_TX_FIFO_OPERATION;
    
    if (HAL_FDCAN_Init(&hfdcan1) != HAL_OK)
    {
        Error_Handler();
    }
    
    // 配置过滤器0：接收目标速度消息(ID: 0x100)
    sFilterConfig.IdType = FDCAN_STANDARD_ID;
    sFilterConfig.FilterIndex = 0;
    sFilterConfig.FilterType = FDCAN_FILTER_MASK;
    sFilterConfig.FilterConfig = FDCAN_FILTER_TO_RXFIFO0;
    sFilterConfig.FilterID1 = CAN_MSG_ID_TARGET_SPEED;
    sFilterConfig.FilterID2 = 0x7FF;
    
    if (HAL_FDCAN_ConfigFilter(&hfdcan1, &sFilterConfig) != HAL_OK)
    {
        Error_Handler();
    }
    
    // 配置过滤器1：接收紧急停止消息(ID: 0x101)
    sFilterConfig.FilterIndex = 1;
    sFilterConfig.FilterID1 = CAN_MSG_ID_STOP;
    sFilterConfig.FilterID2 = 0x7FF;
    
    if (HAL_FDCAN_ConfigFilter(&hfdcan1, &sFilterConfig) != HAL_OK)
    {
        Error_Handler();
    }

    // 配置过滤器1：接收紧急停止消息(ID: 0x101)
    sFilterConfig.FilterIndex = 2;
    sFilterConfig.FilterID1 = CAN_MSG_ID_START;
    sFilterConfig.FilterID2 = 0x7FF;

    if (HAL_FDCAN_ConfigFilter(&hfdcan1, &sFilterConfig) != HAL_OK)
    {
        Error_Handler();
    }
    
    // 启动FDCAN
    if (HAL_FDCAN_Start(&hfdcan1) != HAL_OK)
    {
        Error_Handler();
    }
    
    // 激活接收中断
    if (HAL_FDCAN_ActivateNotification(&hfdcan1, FDCAN_IT_RX_FIFO0_NEW_MESSAGE, 0) != HAL_OK)
    {
        Error_Handler();
    }
    
    printf("FDCAN initialized with industrial-grade message handling\r\n");
}

/**
  * @brief 发送实际速度到F103
  * @param speed: 实际速度值
  * @retval None
  */
void fdcan_send_actual_speed(uint32_t speed,uint8_t direction)
{
    FDCAN_TxHeaderTypeDef TxHeader;
    uint8_t TxData[8];
    
    // 配置发送消息头
    TxHeader.Identifier = CAN_MSG_ID_ACTUAL_SPEED;
    TxHeader.IdType = FDCAN_STANDARD_ID;
    TxHeader.TxFrameType = FDCAN_DATA_FRAME;
    TxHeader.DataLength = FDCAN_DLC_BYTES_5;
    TxHeader.ErrorStateIndicator = FDCAN_ESI_ACTIVE;
    TxHeader.BitRateSwitch = FDCAN_BRS_OFF;
    TxHeader.FDFormat = FDCAN_CLASSIC_CAN;
    TxHeader.TxEventFifoControl = FDCAN_NO_TX_EVENTS;
    TxHeader.MessageMarker = 0;
    
    
    // 将结构体数据拷贝到发送缓冲区
    TxData[0] = (uint8_t)(speed & 0xFF);
    TxData[1] = (uint8_t)((speed >> 8) & 0xFF);
    TxData[2] = (uint8_t)((speed >> 16) & 0xFF);
    TxData[3] = (uint8_t)((speed >> 24) & 0xFF);
    TxData[4] = direction;
    
    // 发送消息
    if (HAL_FDCAN_AddMessageToTxFifoQ(&hfdcan1, &TxHeader, TxData) != HAL_OK)
    {
        printf("FDCAN send failed\r\n");
    }
}



/**
  * @brief FDCAN接收中断回调函数（工业级处理）
  * @param hfdcan: FDCAN句柄
  * @param RxFifo0ITs: 中断标志
  * @retval None
  */
void HAL_FDCAN_RxFifo0Callback(FDCAN_HandleTypeDef *hfdcan, uint32_t RxFifo0ITs)
{
    if ((RxFifo0ITs & FDCAN_IT_RX_FIFO0_NEW_MESSAGE) != RESET)
    {
        FDCAN_RxHeaderTypeDef RxHeader;
        uint8_t RxData[8];
        can_rx_queue_item_t queue_msg;
        
        // 立即读取消息，避免FIFO溢出
        if (HAL_FDCAN_GetRxMessage(hfdcan, FDCAN_RX_FIFO0, &RxHeader, RxData) == HAL_OK)
        {
            switch(RxHeader.Identifier)
            {
                case CAN_MSG_ID_STOP:
                    // 最高优先级：在中断中立即处理
                    fdcan_process_emergency_stop();
                    break;
                    
                case CAN_MSG_ID_TARGET_SPEED:
                    // 只负责设置速度
                    fdcan_process_target_speed_in_isr(RxData);
                    break;
                    
                case CAN_MSG_ID_START:
                    // 1. 在中断中立即处理速度设定
                    fdcan_process_target_speed_in_isr(RxData);
                    
                    // 2. 设置启动请求标志，交由主循环处理
                    g_motor_start_request = 1;
                    break;
                    
                /*
                case CAN_MSG_ID_STATUS_QUERY:
                    // queue_msg.msg_id = RxHeader.Identifier;
                    // memcpy(queue_msg.data, RxData, 8);
                    // queue_msg.dlc = 8;
                    // queue_msg.msg_type = CAN_MSG_TYPE_STATUS_QUERY;
                    // queue_msg.timestamp = HAL_GetTick();
                    // fdcan_queue_put_message(&queue_msg);
                    break;
                */
            }
        }
    }
}
